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I. INTRODUCTION 


The problem discussed in this paper is that of esti- 
mating the position and velocity in two dimensions of a 
target by means of processing  passively obtained bearing 
measurements. 

A single moving observer (tracker) monitors noisy sonar 
bearings from a radiating acoustic source (target). The 
geometric configuration is depicted in Figure 1.l. 

The problem contains nonlinearities so the conventional 
linear analysis is not possible. Also as it will be shown in 
chapter IV the dynamic process remains unobservable prior to 
tracker maneuver. That requirement of observer maneuvering 
distinguishes this problem from the more usual target motion 
analysis (TMA) problem. | 

In chapter II the basic concept of the Kalman filter is 
described. Chapter ТТТ describes the non-linear case 
(Extended Kalman filter) in which category the bearings only 
tracking problem belongs. 

In chapters IV and V the problem of bearings only 


tracking with  nonmaneuvering and maneuvering targets is 


discussed. Some possible solutions from the literature are 
referenced, and the case of solving the problem through a 
Specific approach, i.e by uSing a variable value of the 


system's noise covariance matrix "Q" is tested. 
Chapter VI contains the results of the computer simula- 
tions on the subject and chapter VII contains conclusions 


and possible topics for further investigation. 


TRACKER 





Figure 1.1 Geometrical Configuration for the B.O.T Problem. 
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II. KALMAN FILTERING BASICS 


A HISTORY 


In 1960, R.E. Kalman provided a new way of formulating 
the least squares filtering problem using state-space 
methods [Ref. 1]. Until that time the Wiener solution of 
the optimal filter problem was applied , which was using the 
concept of the "weighting function". In effect the weighting 
function tells how the past values of the input should be 
weighted in order to determine the present value of the 
output, that is the optimal estimate. But the Wiener solu- 
tion did not lend itself very well to the corresponding 
discrete-data problem nor was it easily extended to more 
complicated problems [Ref. 2]. 

The two main features of the Kalman formulation and 


solution of the problem are: 


Vector modeling of the random processes under 


consideration. 


Recursive processing of the noisy measurements 

(input data). The key element in any recursive procedure is 

the use of the results of the previous step to aid in 

obtaining the desired result for the current step. This is 

the main feature of the Kalman filtering and the one that 

clearly distinguishes it from the weighting function 

approach, which requires arithmetic operations on all the 
past data. 

The Kalman filtering technique has become very popular 

in target tracking applications for the previous reasons 


plus the following: 


1 


At a given time t, the filter generates an unbi- 
ased estimate of the state  vector,which means that the 
expected value of the estimate is the value of the state 


vector at time t. 


The estimate 15 а minimum variance estimate 
meaning that it has the property that its error covariance 
is less than or equal to that of any other linear unbiased 


estimate. 


The filter is linear and simplifies the calcula- 
tions [Ref. 3]. 


B. THE DISCRETE KALMAN FILTER 


Assume that the random process to be estimated сап Бе 


modeled in the form: 


x(k+1)= Ф(к)х(к) + Гы(к) + Au(k) (2.1) 


and the observation or measurement of the process is assumed 


to occur at discrete points in time in accordance with the 


relationship: 
Z(k) = H(k)x(k) + n(k) (2228 
where: 


x(k) = (nxl) ; is the process state at time t(k) 


9163 = (пхп) ; is the matrix relating x(k) to 


X(k*1) in the absence of forcing function. 
W(k) ; is the random forcing input at time t(k) 


considered to be an uncorrelated sequence with zero mean and 


known variance. 
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[(k) = (nxp) ; is the matrix relating the random 
forcing inputs to the state at time t(k).! 


u(k) ; is the deterministic forcing input at time 
Esos 


Д (к) - (nxp) ; is the matrix relating the determin- 
istic inputs to the state at time t(t). 


z(k) = (mxl) ; is the measurement vector at time 
46-9 


H(k) = (mxn) ; is the matrix which gives the noise- 
less connection between the state vector and the measurement 


equation at time t(k). 


n(k) = (mx1) ; is the measurement noise error which 
is assumed to De a white sequence with known covariance 


structure and uncorrelated with the w(k) sequence. 


The corresponding covariance matrices are given by:? 


(> (к =: 

Е[ м, м, ] = 2.) 
О " Kx 
Roo кі 

E[n n/] - (207 
O KL 

E[w, n, | = O tor all KK and ı (2.5) 

‘psn 


(') denotes matrix transposition. 
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It is assumed that we have available an initial estimate 
of the process at time t(k), which is based on the knowledge 
about the process prior to time t(k). This prior estimate 
will be denoted as X(k/k-1) where the "hat" denotes esti- 
mate, and the (k/k-1) subscript means that this is our esti- 
mate prior to processing the measurement at time t(k). 

With the assumption of the prior estimate Х(К/К-1), we 
now seek to use the measurement z(k) to improve the esti- 
mate. To do that we choose a linear blending of the received 
noisy measurement and the prior estimate in accordance with 


the equation: 
X(k/k) = R(k/k-1)+G(k)[z(k)-H(k)&(k/k-1)] 284 
where X(k/k) is the update estimate, X(k/k-1) is given by: 
х(к/к-1) -Ф(Ю)%(к-1/к-1) (2.7) 


and the G(k) is the blending factor. G(k) is going to be 
determined later. At this time the problem is to find a 
particular value of G(k) that yields an update estimate that 
is optimal in sce sense. The minimum Mean - Square error 
is the require. performance criterion for that "optimiza- 
tion". To do that we need to define the term "error covari- 
ance matrix" P(k), associated with the update (a posteriori) 
estimate, which is a matrix representing the covariance of 
the difference between the true state vector x(k) and the 


estimated Х (К). 
2 | 
P(k) » E [(x(k)-X(k/k-1)) (x(k)-X(k/k-1))' ] (2.8) 
The optimization can be done by various mathematical ways as 


treated in [Ref. 4] [Ref. 2] and [Ref. 5]. The mathematical 


derivation which is omitted here shows that if 
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G(k)zP(k/k-1)H' (K) [H(k)P(k/k- 1)H' (k) E (2.9) 


then this is the G(k) that minimizes the mean square estima- 
Exon error, and it is called the "Kalman gain" [Ref. 2]. 
Next the covariance matrix associated with the optimal 


estimate may be computed and is given by:? 
P(k/k) = [I - G(k)H(k)]P(k/k-1) (2210) 


Now the updated estimate x(k/k) can be easily projected 


ahead via the transition matrix by the equation: 
х(к+1/к) = Ф(к)х (к/к) (ӘЛИ) 


ignoring the contribution of w(k) because it has zero mean 
and also it is uncorrelated with the previous W's. 


Also,the equation 


Р(к+1/к) = Ф(к)р(к/к)ф (к) +9(к) (2.12) 


closes the loop and now, having the needed quantities for 
the next moment with the next measurement we can start again 
as in the previous steps. 

ит лоп5 (2.6), (2,9), (2,10), (2,11), апа (2,12) thus 
comprise the Kalman filter recursive equation set. 


In Figure 2.1 the Kalman filter loop is indicated. 


1. A Simple Example 


Assume that a stationary tracker has the ability to 
obtain range measurements in both X and Y directions of a 


target moving as in Figure 2.2. 


(I) is the identity matrix. 
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Compute Kalman gafn: 
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Update estimate with 
Measurement 2(k) 
(k/K)ex(k/k- PEOR 
HCK)x(k/k-1)] 


Project enead: 
X(kel/k )2O(k)x(Kk/k) and 
Ы JO' (K)*Q 
k 













Compute error covariance for updated 
estimate: P(k/k)-LI-GCOk)H(OC )PCKk/k- 0) 





Figure 2.1 Тһе Kalman Filter Loop. 


Let the target be moving with a tangential velocity of 
1,660 m/min so that it covers the arc of 90 in 10 minutes. 
The tracker makes its measurements every l min. It is 
desired to estimate the state vector of the target defined 
ав Х,Ух,У, and Vy , i.e., тапре са е сосу е 
directions. Given are: an initial estimate X(k/k-1) and its 


error covariance matrix P(k/k-1). Let them be: 
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10000 


^ 
x(k/k h) о (2 7 
O 
(600 
апа 
1000 0 0 O 
0 1000 о О 
P(k/k-1) = | (27124) 
О 0 1000 o 
о O о IOO O 
Then we can calculate the Kalman filter gain G(k) as in 


equation (2.9) where: 


/ O O O 
H(k) = constant = (277150 
о O / о 
and R(k) has the value: 
| 0 
R(k) = constant = (2216!) 
о 1 


Next, given the measurement, the updated estimate is calcu- 


lated using equation (2.6) where: 
z, 
(KS (2.17 
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We can see here that the updated estimate x(k/k) depends on 
the previous x(k-1/k-1) propagated for the instant (k) i.e., 
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X(k/k-1), and another portion equal to G(k)[z(k)-H(k)x(k)]. 
That second portion depends on the G(k) and on how much the 
estimated and the received measurements differ. 

The updated error covariance matrix P(k/k) is then 
computed using equation (2.10). The updated error covariance 
P(k/k) is going to be less than the previous P(k/k-1) since 
the filter processed an observation and thus the uncertainty 
about the estimate is less. The term [I-G(k)H(k)] is always 
less than unity if G(k) is not zero. That means that if we 
used the last observation (i.e., G(k) not zero) then the 
term in the brackets is less than unity and Р(К) becomes 
less than P(k/k-1). 

Now having X(k/k) and P(k/k) we must propagate them 
for the next instant when the next measurement will be taken 
in order to be able to compare it with the real one through 
the new measurement. So we project ahead our estimate by 


the equation (2.11) where: 


| / O O 
O о O 
фо) оо ^ ! (2.18) 
/ 
M "d 


and the error covariance matrix by equation (2.12) with Q(k) 
such that: 


0(к) = ['GOE[wCk)w' CK) J ' CK) 08920 


where: 
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аа | о 3 (2.20) 


and w(k) is the random forcing input at time t(k) which is 
to be formulated as а white noise with known variance. 


Finally Q(k) is given by: 


4 5 
2 Ao о 
4 L 
3 
Q(k) = т“ T. (2 2298 
0 ٥ 4 2 
3 
0 O ДІР i 
a 


and it counts for the uncertainty introduced by the fact 
that we do not know if the target during the next coming 
time interval will maneuver or not. A big value of w(k) 
means that the target is very likely during the propagation 
interval to maneuver. In this case the Kalman gain will also 
be large andthe filter will weight the observation more 
than the propagated state. On the opposite case if w(k) is 
zero the filter assumes that the target did not maneuver 
during that interval so it weights more the last estimate 
than the measurement. In the above case it is also assumed 
that the target acceleration in xX direction is uncorrelated 
to the acceleration in Y direction for simplicity. 

Having the propagated values of x(k+1/k) and 
P(k+1/k) we can start over again from the initial step. 

The above algorithm was simulated in the computer. 
The interesting result obtained is that for the case that 
the target maneuvers the choice of w(k) is very important. 


If it is small or zero the filter does not include any extra 
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uncertainty due to possible target maneuver. So at any 
moment it gives more weight to the last propagated estima- 
tion and less to the received measurement. Thus the tracking 
accuracy is not good compared with that in which it includes 
uncertainty as can be seen in the results shown in Figures 
+ 2.5, 2.6, апа 2.7. 
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Fipure 2.3 Filter Behavior for w=0.0. 
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Figure 2.5 Filter Behavior for w-1.0. 


24 


00 


Y (RANGE) 


6000 


14000 


12000 


8000 


4000 


LEGEND 


= A œ ہے‎ = = M =“ = = ш шт S. 


2000 





0 2000 4000 6000 8000 10000 
X (RANGE) 


Figure 2.6 Filter Behavior for w=3.0. 


25 


— — - — s í 


Y (RANGE) 


4000 6000 8000 10900 12000 14000 


2000 


LEGEND 


o KEAL- Е 
© ESTIMATED ` 


2000 


Figure 2.7 


PROJECTED 


- т e Po o цы з= з= ак ае ае. -. چت‎ 


4000 6000 
X (RANGE) 


Filter Behavior 


26 





8000 10000 


for w-10.0. 


III. NONLINEAR ESTIMATION 


A. INTRODUCTION 


The majority of physical phenomena are nonlinear in 


nature. So as a result ,usually ,the state and/or measure- 
ment equations are nonlinear. Since the basic Kalman filter 
theory deals with linear cases, it is necessary to find a 


"method" to use it in nonlinear estimation problems. 
There are two ways of solving that problem:  [Ref. 4]. 
1. By deriving an optimal filter for the nonlinear 
problem or 
2. By linearizing (approximating) the problem and 
applying the linear filter theory. 

The first method is hard to follow and will involve 
complicated mathematical computations. On the other hand the 
Second method is easier and the more usual. For the 
reasons above the second method will be followed in this and 


the following chapters. 


B. ANALYSIS 


In the following analysis it is assumed that both the 
State and the measurement equations are nonlinear although 
"this is not always the case. 

Assume that the random process to be estimated can be 


modeled by: 
X(k*1) » a[x(k),u(k),k] *w(k) رت‎ 
with the measurement equation: 


ӨШ) - с(х(Е))| піск) (3.2) 
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It is necessary to have available a nominal trajectory 
x (k), k-0,1,2,.... about which the linearization will be 
performed. The vector function a[x(k),u(k),k] is expanded in 
Taylor series about the nominal trajectory x (Kk). Then the 


linearized state equations can be written: 


x(k«l)ca[x 9 (k),u(k),k]« 9a | [x(k)-x (k)]ew(k) (3.3) 
X 
[x (k),u(k),k] 


If A(k) is defined to be the first partial derivative of the 
nonlinear function a[x (k),u(k),k], with respect to the 


state vector x(k), i.e., 





A(k) = (3.4) 


% [ су, ц (к), к] 


Then, the ijth entry of matrix À is piven by: 


в о e 


axes (3/5) 


E [x ^X ky, (k), ] 


Also ,the vector function а(х € (k),u(k),k] is a known 
function of k. Thus the linearized state equations can be 


written as: 
x(k+1)=A(k)x(k)+a[x®(k),u(k),k] (3.6) 
- A(k)x (9(k) *w(k) 
The accuracy of this approximation depends on how close 
the nominal trajectory to the actual one was selected. 
Let us now consider the measurement equation. We have: 
2(К) = c [x (k)] *n(k) (3.7) 
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Again we can expand the nonlinear vector function c about 


the nominal trajectory x (k) with the result: 


z(k)ec[x ? (k)]* 9e || [xGO-x 9(0]*n(k) (3.8) 
X 
х (° (к) 


Defining 


a с 


НК) = 2 
x (9? y) 





we can write: 
z (k)-H(k)x(k)*c [x 9 (k)) -H(k)x ? (kx) n(k) (20100) 
Again as in the linearized state equation, the two terms in 


the middle of the equation (3.10) аге known quantities and 


they can be handled as if they were a time varying but known 


measurement bias. For simplification if we will define* 
u'(k) = а[х ÜO(k),u(k),k] - A(k) x ?(k) (3.11) 
and 
z'(k) - z(k)-c [x'? (k)] *H(k)x ? (k) » z(k)-A(k) (5232) 
where: 


t 


“(') in this case means "prime' 


29 


з (к) = с ОО - H(k)x(k)] (3.13) 


we can rewrite equations (3.6) and (3.7) as: 


x(k+1) = A(k)x(k) + u(k) + w(k) (22147) 
апа 

z(k) = H(k)x(k) + #(k) * n(k) (ЭОЕ) 
Then starting with these linearized equations, the appro- 


priate Kalman filter equations are: 


the gain equation: 

G(k) - P(k/k-1)H' (k [H(k)P(k/k-1)H' (k)*R(k)]- (316) 
the covariance of estimation error equations: 

P(k/K-1) » A(k- 1)P(k-1/k-1)A' (k- 1) *Q(k-1) 5:179) 

P(k/k) = [I-G(k)H(k)] P(k/k-1) (3.18) 
the filter update equation: 


X(k/k) = X(k/k-1)*G(k)[z(k)-c(X(k/k-1))] (3.19) 


and the prediction equation: 
х(к+1/к) = a[x(k/k),u(k),k] (3.20) 


Notice that in equations (3.19) and (3.20) the nonlinear 
State and measurement relationships are used. An alternative 
is to use the linearize relationships in which case ме 


have: 
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(к/к) = X(k/k-1)*G(k)[z(k)-H(k)X(k/k-1)] (3221) 
and 
X(k*l/k) = A(k)X(k/k) * u(k) (322223) 


One question to be answered now is how to determine the 


"nominal" trajectory used before. One way is to use an 
approximate trajectory that is known in advance. This 
trajectory may be available from known data, or may have 


been computed by solving the state equations: 


Ak) = alx (k) ulk), k] (D 


with the initial condition х 9(0) = E[x(0)]. Unfortunately, 
if the uncertainty in x(0) is large the solution of equation 
(3.23) may be "too far" from x(k), the linerization error 


too big and the whole method inaccurate. 


C. THE EXTENDED KALMAN FILTER 


Another possibility is to use the estimates produced by 
the filter as the nominal trajectory about which the linear- 
ization is performed. The estimator equations are again 
given by equations (3.21) and (3.22). The matrices A(k) and 
H(k) must be used to generate G(k) зо that it is available 


to process z(k) when it is available. Thus the best informa- 


tion we have when H(k) must be evaluated is Х(К/К-1): when 
A(k) is to be evaluated, however, X(k/k) is available. 
Hence: 


1 


om (3.24) 


A(k) =. 

x 
[Ж (к\к) ju 60, к) 
and 

H(k) side (3 253 

jx 
| Х(618-0)| 
The H(k) and A(k) matrices must be computed on-line 


and not in advance since they depend on the last estimate. 
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IV. BEARINGS ONLY TARGET MOTION ANALYSIS - NONMANEUVERING 


一 一 一 一 
سد‎ Аса... 


TARGET 


A. PROBLEH DEFINITION 


The problem considered here is that of estimating the 
position and velocity of a target, in two dimensions, Ьу 
processing Passively obtained bearing measurements. 

The main application area is the  Antisubmarine Warfare 
area where either a surface ship tries to locate a submarine 
through its cavitation noise or sonar transmissions, or vice 
versa. 

In the following discussion we will consider a moving 
observer (own ship) that monitors noisy sonar bearings to an 
acoustic source (target), and processes these measurements 
to obtain estimates of target position and  velocity.The 


geometric configuration is Shown in Figure 1.1. 


B. FORMULATIONS OF THE PROBLEM 


As it was mentioned earlier the problem contains nonli- 
nearities, andthe linear Kalman filter is not applicable. 
Depending on the selection of the working coordinate system 
the nonlinear term may be either the state equation or the 
measurement equation. Even models with mixed elements from 
different coordinate systems have been used. Following are 


the most commonly used formulations of the problem: 
1. Modified Polar Coordinates 


In the modified polar (MP) coordinates the state 
vector is comprised of the following components: 
| Bearing 
Bearing rate 


Range rate divided by range 


33 


The reciprocal of range. 
In this case the measurement equation is linear and the 
state equation nonlinear. The nonlinearities exhibited by 
the state equations are considerably more complicated than 
those exhibited by a formulation where the measurement equa- 
tion is the nonlinear. Consequently the computational load 
for this formulation is increased. Details about the modi- 


fied polar coordinates formulation can be found in [Ref. 6]. 
2. Mixed Coordinates 


In this case as in the previous one the 'Surement 
equation is the linear one and the state eq. ion поп 
linear. The state vector consists of: 

Bearing 

Range 

Velocity component in x-direction 

Velocity component in y-direction 
Again in this formulation there 15 the same complexity in 
linearizing the state equation as well as computational 
load. Analysis of the mixed-coordinate formulation can be 
found in [Ref. 7]. 


Pseudo-Linear Formulation 


This formulation involves replacing of the measured 
bearings with  pseudo-linear measurement  residuals, to 
decouple the covariance computations from the estimated 
solution. The attractive feature of this method is that it 
permits a solution to the problem via linear estimation 
techniques. This formulation is similar to the Cartesian 
formulation which will be discussed in the next subsection. 
How does it differ from the Cartesian formulation can be 
found in appendix D. More details on this formulation can 
be found in [Ref. 8]. 
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^. Cartesian Coordinates Formulation 


This is the traditional way of  formulating the 
problem. The state vector consists of: 
1. Range in x-direction 
2. Velocity component in x-direction 
3. Range in y-direction 
&. Velocity component in y-direction. 
The state equation is linear and the measurement equation is 
now the nonlinear part. However the exhibited nonlinearity 
is easily circumvented without complicated or lengthy compu- 
tations as it will be shown in the next section. 
Finally the cartesian coordinate formulation will be 
adapted in the following discusion mainly because of 16$ 


Simplicity . 


C. DESCRIPTION OF THE FILTER IN CARTESIAN COORDINATES 
1. Derivation of the State Equations 


If we will consider the geometric configuration of 
Figure l and with the restriction of target and tracker 
being in the same horizontal plane, the Cartesian formula- 
tion state vector may contain relative ranges and relative 
velocities in X and Y directions. The state vector that will 


be followed in this analysis is: 


x, (t) x(t) 
ӨТСЕ?) : МӘСЕ) (4.1) 
ВЕ: ЖЕ) 
А шд 


with: 
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x(t) Xp Gib) = (Е) 


(4.2) 
Vx (t) ыы 
UE) Ye Cl) avenues 
vy (t ) Уу (t )-Vyo (t) 


where x.,(t).y لی‎ ٤٣٤١٠٢4 +8807 are the target absolute 
components of position and velocity in X and Y directions, 
and x,(t),y, (t) veo (t), and v,, (t) “are the ص8‎ 
components of position and velocity. The linear differen- 


tial equations of motion of the model are given by: 


e 
xe (t xg (E) 
ха (Е) | _ | а„(Е) (4.3) 
хз (Е) | | ж (Е) | 
хү (| зада? 

with: 
ay (t T" аста оге) (4.4) 
ay (t) am t) ao 


where a,(t) and a(t) are the relative accelerations ВоВ 
directions, and а, (=) а Сос t p DONE ы are the 
corresponding absolute  accelerations of target and tracker 
in both directions correspondingly. 

The solutions of the differential equations above in 


matrix notation give: 


x(t) = A(t,,tO)x( CO О (4.5) 
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with: 


1 (t-tO) 0 0 
27 0 1 0 | 0 ПЕС, 
(ON. 0 0 1 (t-tO) 
0 0 0 1 
and: 
t 
u, (t,t0) Ne-a x Ода) 
t 
EEO EROT 
А = з | 
Cu) 
i ӨЛІГІ Де-Маў (аж 2 
7 
uy (t,t0) 0“ ۲ 
te 
and (t0) denotes any arbitrary fixed value of time. 
Although (4.5) is valid for unconstrained vehicle 


motion, solution requirements necessitate that the bearings- 
only target motion analysis be formulated under the restric- 
tive assumption of constant target velocity.  [Ref. 9]. In 
this case ay, (t) and a,4 (t) become zero and u(t,t0) reduces 


to a deterministic input vector which depends only upon the 


tracker's acceleration (maneuvers). So 
ma 0 = Qu, (t;,t0) (4.8) 
where: 
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í ] 
ug, (t, t0) а-а, 0942 (4.9) 
doa (Е, ЕО) Saxo (А)ал 


Че (+4. = А 
Uo, (t, t0) 122727 (3)42 
49 
+ 
Чо (Е 140) а d 
Saro (DAA 
2. Derivation of the Measurement Equation 


The measurement process is described by a scalar 


time varying equation of the form: 


B(t) < 8880 (4.10) 
where 
h[x(t)]= arctan xs,(t)/xi (t) (&.11) 


and $(t) represents the measured target bearing corrupted 


by additive measurement noise n(t). It is assumed that n(t) 
is a white noise with zero mean and known variance Ow 
i.e., 
ENCE TO (4.12) 
апа 3 
б A29 
E[n(t)n(t*2)] = (л) 
о А Zo 


р. THE DISCRETE C PIMERIIOPEE 


: The previously defined model in discrete form is 


described by: 
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а). - A(k)x(k) - uk) (4.14) 


апа 


8 (к) = h[x(k)] + n(k) (4.15) 


where: 


A . | 
x(k) is the (4х1) state vector consisted from rela- 


tive range and velocity of the target in X and Y directions. 


A(k) is the (4x4) state transition matrix which is 


constant and given by: 


1 T 0 0 
0 1 0 0 
AGO = (4.16) 
0 0 1 T 
0 0 0 1 
J 
u(k) is the (4x1) vector of deterministic inputs 


due to tracker's movement and given by Equation (4.9). 


必 (k) is the scalar noisy bearing measurement taken 
at time t(k). 


n(k) is the scalar additive measurement noise at 
time t(k). 

Equation (4.14) assumes that the target moves with zero 
acceleration, (non maneuvering). Also it is assumed that the 
additive measurement noise n(k) has zero mean and a known 
variance o67(k). Finally an initial estimate of the state 
vector and its error covariance matrix is presumed to be 


given. 
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The extended Kalman filter technique is applied to the 
problem and yields: 


A 
x(0/0) is the initial estimate of the state vector 


which is considered to be given. 


P(0/0) is the initial estimate error covariance 


matrix which is also considered to be given. 
%(k/k-1) = A(k)x(k-1/k-1)-w(k) (4.17) 
is the projection ahead of the estimated state vector. 
P(k/k-1) = A(k)P(k-1/k-1)A'(k) * Q(k) (4.18) 


is the projection of the error covariance matrix and Q(k) is 
the maneuver excitation covariance matrix (assumed zero if 
the target does not maneuver). 


Sh 


cjx 





x-X(k/k-1) 


is a (1x4) matrix given by: 
Н(К)= [x4/(x2*x2),0,-x, / (x2*x2),0] (120) 
x=x(k/k-1) 
G (Ic) =P (e/k- 1)H" (ik) [H(k) P(e/k-1)H' (ke) +02 (ke) J (4.21) 
is the gain equation 
х(к/к) - X(k/k-1)*G(k)[£(k) -h£(k/k-1)] (4.22) 
is the update equation and 
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P(k/k) » [I-G(k)H(X)]P(k/k-1) (4.23) 


is the error covariance update equation. 

The above algorithm was formulated in computer simula- 
tion program as in Appendix B and tested for the situations 
shown in Figure 4.1 and Figure 4.2. 

In the first case (Figure 4.1),the target was moving 
from east to west on a constant course and speed of 20 
m/sec, while the tracker was maneuvering following a sinuso- 
dial track with main course from east to west also, anda 
velocity of 10 m/sec in x-direction. The target had a rela- 
tive velocity of 10 m/sec with reSpect the tracker in the 
X-axis and O m/sec in the Y-axis. 

In the second case (Figure 4.2), the target was moving 
as the first case but the tracker was following a circular 
path of radius 2000 m with a turning rate of 2°/sec. 

The measurement error was taken as zero mean апа 0.1 
covariance and the measurement interval 1 sec. The behavior 
of the filter is displayed in the following Figures and is 


considered to be satisfactory. 
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V. MANEUVERING TARGET 


Up to this time we made the assumption that the target 
does not maneuver. However in real world applications this 
15 not the usual case and hence the assumption is unreason- 
able. Specifically, in the main application area of the 
bearings-only tracking,  i.e., in the A.S.W scene, jt. 349 
expected that the target will not keep constant velocity but 
instead it will command some kind of zig-sag during the 
normal open sea transit and strong maneuvering or evasion 
after detection of a potential threat. It is evident thus 


that there is a need to accommodate the maneuvering case. 


A. POSSIBLE APPROACHES 


There are various approaches relative to the problem in 


general. Some found in the literature are following: 
1. Variable Dimension Filter 


In this case, the filter operates in its normal mode 


in the absence of any maneuvers. A detection scheme is used 
to determine that a maneuver is indeed occuring. Once a 
maneuver is detected, a different state model is used. The 


extent of the maneuver as detected is then used to yield an 
estimate for the extra state components. The tracking is 
then continued with the augmented state model until it will 
be reverted to the normal model by another decision. The two 
models are a constant velocity and a constant acceleration 
model for the maneuvering сазе. Details on the analysis of 
that method can be found in [Ref. 10]. 
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2 Expanded Number of States 


In this case the model includes the acceleration 
component in it. This method has the disadvantage that if 
the target does not have acceleration, using a third order 
model increases the estimation errors for both position and 
velocity [Ref. 10]. Also,the computational load increases 


drastically by augmenting the model by one term. 


J. Modeling Target Acceleration as Random Process of 


Known Form 


This method is based on the fact that the target 
acceleration and thus the target maneuver, is correlated in 
time; i.e., if the target is accelerating at time t, it is 
likely to be accelerating at time t*&tau for sufficiently 
small €. А typical representative model of the correla- 
tion function r( ) associated with the target acceleration 


is given by: 


-dl?| | 
r(e) - E [a(t)a(t*?)] = б'е | ‚а>0 052019) 
where (67) is the variance of the target acceleration and 
(a) is the reciprocal of the maneuver time constant. The 


maneuver excitation covariance matrix Q(k) then depends on 
the correlation function r(@),which also depends on the type 
of the target. 

The above formulation includes the acceleration term 
in the state vector. So the performance of the filter is 
degraded by the computational overhead. The quality of the 
estimate is also degraded when the target iS moving with 
comstant velocity. 

Analysis of the above method can be found ас 
[Ref. 11]. 
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4. Use Variable Maneuver  Excitation Error  Covariance 


RR اا ل س‎ -一 一 一 -一 一 一 一 一 


Matrix 


The filter is modeled as a second order and it does 
not include acceleration term in it. The idea is [Ref. 12]. 
to use a set of different values for the forcing input 
covariance Q(k). The filter monitors the innovation error 


in the equation: 
х(к/к) - x(k/k-1) * G(k)[É(k) - h(Z(k/k-1))] (5.2) 


i.e., the term [$(k)-h(X(k/k-1))] in every iteration. If 
that error becomes larger than a predetermined threshold, 
that means that the received bearing measurement does not 
agree with that the filter generated and was supposed to 
receive. Correspondingly, the estimated vector does not 
agree with the actual. So the filter assumes that the target 
made a maneuver. Depending on the size of the innovation 
error, a value for the excitation covariance matrix Q(k) 15 


applied to the error covariance propagation equation: 
P(k/k-1) -» A(k)P(k/k-1)A'(k) + Q(k) (5:39 


The effect of the above is to increase the uncertainty of 
the filter which consequently causes an increase of the gain 
G(k). The bigger the G(k) the more the filter "believes" 
the measurements rather than the previous estimates. 

So the filter is "partially" reinitialized. By 
partially is meant that the new initial estimates of the 
state vector and specifically the range terms are very close 
to the real ones estimated just before the maneuver. Thus 
the filter has good conditions to start over and estimate 


the new state after the maneuver. 
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B. BEARINGS-ONLY TRACKING WITH MANEUVERING TARGET. 


From the previously mentioned methods of dealing with 
maneuvering targets, we are going to develop the last one 
i.e., that of using a variable maneuver excitation error 
covariance matrix Q(k). 

This method uses a four-state model, so it is faster 
than the others using a six-state models and is the simplest 
of all. Actually only a few extra lines of program are 


added to that of a nonmaneuvering target. 
l. Determination of the Q(k) Matrix 


If we will suppose that the target made a maneuver, 
(acceleration (a)) during the state propagation time from 
(К) to (k+l), in one direction say X, then the error intro- 
duced to our propagated estimate in the range term will be 
(1/2)aT? and the error introduced in the velocity term will 
be aT. Combining that fact in both direction and with the 
assumption that an acceleration in X is uncorrelated to an 


acceleration in Y the resulting Q(k) is given by: 


Q(k) = [ (k)E[w(k)w'(k)][' "ОЮ (3.4) 
where: 
е 
а, 
E 
Г(к) з (5.5) 
O 
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2. Simulation Results for Cases 3 and 4 with м-1,6:-0.1 


— — — — 2 2 2 2 2 2 2 2 7 Š —  — í — لے‎ | OH FANITH.— — ——Á سس ا —— ` ل‎ 


The above method was modeled and simulated in the 
computer. Two geometric configurations of target and 


tracker as shown in Figures (5.1) and (5.2) were tested. 
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In the case 3 the target was following a steady course from 
east to west and a constant speed of 20 m/sec. At the 700th 
second it changed course to the right and speed components 
to 12 m/sec in X and -10 m/sec in Y direction. After 
another 700 seconds it changed course again that time from 
west to east and resumed a speed of 20 m/sec. The tracker 
was moving as in case 1 of the nonmaneuvering target. The 
intervals displayed in the Figures (5.1) and (5.2) согге- 
spond to time intervals of 100 seconds. 

In the case 94 the target was following the same 
track as in case 3 but that time the tracker was maneuvering 
as in case 2. 

In the following simulations the measurement error 
was supposed to have zero mean and 0.1 variance. The meas- 
urement interval was again taken as 1 зес which is also 
considered as reasonable for a real application.The (W) was 
taken equal to 1.0. 

The simulation program (Appendix C) for the above 
conditions gave the results shown in the following Figures 
5.3 to 5.16. In both cases the filter detected the maneuver 
and very rapidly after approximately 300 seconds estimated 
the new target parameters. The fluctuations of the errors 
due to the target maneuver are smaller than those during the 
first initialization of the problem. This can be explained 
by the fact that after the target maneuver detection the 


t 


"filter had an "accurate" reinitialization state from the 


previous tracking. 
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3 Filter Behavior Under Diftfewen уай ез о ur сх 


 — e M k ———N M—MA— F T инк. ee 


In order to investigate the behavior of the filter 
for more extreme conditions, simulations where conducted 
with various values of measurement error variance (€?) апа 
various values of (w). The following combinations where 
tested for the case 3 configuration and the range error was 


obtained in each of the combinations. 


wW 2 

5ت0 0 

20 

0 4.0 

0.5 

.0 20 

0 &.0 

3 0.5 
0 2 

4.0 

10.0 0.5 

10.0 EO 

10.0 4.0 


In the following Figures 5.17 to 5.28 the filter 
behavior is displayed. It is characteristic that the filter 
tracking accuracy and quality is related to both values of 
(w) and (6°). For the specific configuration it came out 
that if the (6?) was more than 0.5 then the filter was very 
sensitive to the value of (w). The best results were 
obtained with the smallest tested value of w=0.1. This 
should be expected because in the case that the measurement 


noise is too big and we additionally introduce uncertainty 
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due to the target maneuver, then the filter assumes a lot of 
uncertainty and at any time behaves erratically following 
the noisy received measurements. It seems that for each 
kind of target and environmental condition (i.e. measurement 
noise variance) there will be an optimal (w) to account for 


the target maneuvers. 
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VI. CONCLUSIONS 


The proposed way of solving the problem of tracking a 
maneuvering target using noisy bearings-only measurements 
was tested and it exhibited satisfactory behavior. The main 


characteristrcs of it are: 


1. The filter responds satisfactorily in the case 
that the target maneuvers. The filter appears to be very 
sensitive to the value of o?. The results were satisfactory 


up to the value of o*^-0.5. After that the behavior of the 
filter depends very much on the value of w. The smaller the 


value of w the Detter the filter tracks. 


2. The design is simple and almost no extra compu- 
tational power is needed beyond that of a nonmaneuvering 


target filter. 


3. The estimation is accurate for a nonmaneuvering 
target as well, and it does not pay the overhead of reduced 


accuracy as the other methods do in the nonmaneuvering case. 


4. Some other target-tracker configurations were 
tested which are not referenced in the previous chapters. In 
some of them the filter exhibited disability to track the 
target. In those cases the characteristic event was that the 
target was moving in such a way that even the tracker's 
maneuvers did not cause significant changes in the measured 
bearings. So the tracker maneuvers are very important in the 
bearings-only tracking problem. They must be such that will 
cause changing bearing rates. Of course the tracker's 
maneuvers are restricted by various factors as speed capa- 
bility, tactical situation, intentions (evade or attack), 


etc. 


94 


Possible subjects for further investigation: 

l. Analytically how does the filter behave ina 
variety of tracker-target, w - œ? configurations? For 
example, for a given value of (6?), what is the optimal (w)? 

2. In the simulations the tracker was supposed to 
move with a continuously changing course which is not the 
real case. Also the tracker was supposed to assume nuge 
amounts of acceleration during its maneuvers, i.e. it was 
supposed to change course and speed in one second which also 
is not realistic. How does this assumption differ from the 
real case? 

3. Investigate the tracker motion under realistic 
constraints with the requirement of obtaining tactical 
advantage and simultaneously providing needed bearing rate 
to accurately solve the tracking problem. 

4. Investigate the effect of assuming realistic 
constraints on target motion. 

In this Thesis we dealt with the problem of maneuvering 
target passive tracking uSing a simple method. The first 
results are satisfactory, however the method needs further 


detailed investigation for even better performance. 
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APPENDIX A 
SIMPLE EXAMPLE SIMULATION PROGRAM 


REAL™*4 P(4,4),H(2,4),HT(4,2),F(4,4),FT(4,4), 

и 
“Х(4,1),Т,ТТ,2(201 |) ХРЕ(5,1уУРРЕ(46 670002) 9 ОШ 

6 (4,2 
%ҺХ(2,1),2НХ(2 1) ,61(4,2),52(2 27 66 ЕНТ 
*FX(4,1),R1,R2,X1(4,1),R(2,2),W,FPUP(4,4), FPUPFT(4,4), 
*A1,A2,A3,R3 

INTEGER N,M,KK,I,J,K,L,NR,S 


N 

W=0.5 

M=1 

S=2 

NR=15 

DO ЁО 7٣۲٣7 
0 ۳٦ 
FCI. J)O 
EFO DSN 
F(1,2)=1. 
F(2.2)-15 
F(3,3)=1. 
F(3,4)=1. 
F(4,4)=1. 
DO 2 I=1,N 
ро 2 del. 
FP(I 7 
DO 3 I=1,N 
DO 3 J=1,N 
- 9پ‎ ٤ 
DO 4 9۴ 
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71 


DO 4 J=1,N 
57۲0(7 

х (1986)- 10000. 
ОШО. 
Х(3,1)-0. 
Х(4,1)-1600. 
R(1,1)-1. 
Rel. 2)=0. 

9 111-7 
МОРТ 

DO 5 I=1,N 
DO 5 J=1,N 
P(I,J)=0. 
P(1,1)=1000. 
P(2,2)=1000. 
P(3,3)=1000. 
Р(4,4)-1000. 
DO 6 І-1,5 
DO 6 J=1,N 
H(I,J)=0. 
абавя 
۹۷۹۷۳ 

Do TIS 
БИ 1 
RS I) (I J) 
DO 71 I=1,N 
DO 71 J=1,N 
Q(I,J)=0. 
Q(1,1)2.25*W 
Q(3,3)2.25*W 


Q(1,2)=.5*W 
Q(3,4)=.5*W 
Q(2,1)=.5*W 
Q(4,3)=.5*W 
Q(2,2)=W 
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10 


rT 


Q(4,4)=W 

DO 999 KK=1,NR 

T=FLOAT (KK) 

0 ھ8 

ТТ-ЕПШАН(Е) 
Z(1,1)=10000.*COS(.157*TT) 
2(2.1) ° 10000 о TT) 
CALL MM(P,HT,S1,N,N,S) 
CALL MM(H,S1,S2,S,N,S) 

DO 8 I=1l,S 

DO 8 J=1,S 
вт) 

РЕТ В 3900000 ME 
568) 2 7 

S6(L 2J ٦ 
S6(2,1)=-S2 (2,1) / DET 
S6(2;,2)-S2(1,1)7DET 

CALL MM(S1,S6,G,N,S,S) 
CALL MM(H,X,HX,S,N,M) 

DO 9 I=1,S 

DO 9 J=1,M 

2 人 гаса нера) 
CALL MM(G,ZHX,XI,N,S,M) 

DO lB TIN 

DO 10 J=1,M 
KORIE K(i J) (т) 

CALL MM(G,H,GH,N,S,N) 

DO 11 I-1,N 

шигээ 

ОМОГ Сат) 
CALL MM(IGH,P,PUP,N,N,N) 
CALL MM(F,X,XPR,N,N,M) 
CALL MM(F,PUP,FPUP,N,N,N) 
CALL MM(FPUP,FT,FPUPFT.N,N,N) 
DO 13 I=1,N 
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0009 А 


13 ERR Gio PUPPET (Lk. От, т) 
КЕШЕ (657 106)2(1.1) ӨШ) Х(1,1),Х(3,1),ХРЕ(1,1), 
*XPR(3,1) 


X(1,1)=XPR(1,1) 
X(2,1)-XPR(2,1) 
X(3,1)=XPR(3,1) 
X(4,1)=XPR(4,1) 
DO 14 I=1,N 
DO IN 

14 P(I,J)=PPR(I,J) 

106 — FORMAT(6(F8.1)) 

999 CONTINUE 
STOP 
END 
SUBROUTINE MM(A,B,C,N1,N2,N3) 
REAL*4 A(N1,N2),B(N2,N3),C(N1,N3) 
DO 100 I=1,N1 
DO 100 K=1,N3 
С(1,К)-0. 
L J EN? 

100  C(I,E)-C(I,K)*A(I,J)*B(J,K) 
RETURN 
END 
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APPENDIX B 
B.O.T NONMANEUVERING TARGET SIMULATION PROGRAM. 


REALS P(4,4),H(1,4),HT(4,1),Q(4,4) ,AX, 
2A (1, 1), YAGI) 
*RI(1,1),VV(4000,4) ,RR(4000) ,S7(4,4), 
“СЕТ(А А) Хана 2 22011) 

УС, ют, ще 
*s2 (1,1) KIA 1), TI kI OEI UUN 
*HH(1,1),HI(4,1),X(4,1),6(4,1), 
586(1:1)(6Ү( Е.Н TIT TL, 

ХӨНХ(5 Е (А ЛА) XPR(4 1). 
*PPR(4,4),FP(^,4),GT(1,5),R(1,1), 
*GH(4,4) ,IGH(4,4) ,IGHT(4,4),PUP(4,4), 
*IGHP(4,4),IGHPT(4,4),GR(4,1) 

INTEGER N,M,NN,NR,L,KK,I,J,K,NS 


N=4 

M=1 

NN=1 
W=3.D0 
NR=2000 
NS-4000 
ope pu 7.209719 
SX-50. DO 
SY-50. DO 
SVX-l. DO 
SVY-l. DO 


DS=211133.D0 
05 1 7:  0|>ہ‎ 
HHH= 0. DO 


DO 1 J=1,N 


100 


10 


Jut 


12 


C 
C 


C 


DO 1 I=1,N 
О(І,7)-0.00 
F(I,J)=0.D0 
F(1,1)=1.D0 
F(1,2)=1.D0 
ОЛ ШО) 
F(3,3)=1.DO 
F(3,4)=1.D0 
F(4,4)=1.D0 
00 4+ 7٤7 
DO 2 J=1,N 
۲۶۶۲٢ 1) 


GENERATION AND STORAGE OFF I.C.NOISE 
DO 10 I-1,N 

CALL GGNML (DS,NS,RR) 

DO 10 J=1,NN 

VV(J,I)sRR(J) 


MAKE MATRIX S7=IDENTITY. 
00 ۱ I N 
DO 11 J=1,N 
57 (1,7)=0.ро 
DBO м 
09ت“‎ 6 


OPT SI IIS FIRST RUN, INITIAL STATE VALUE. 


DO 99 JJ=1,NN 
X(1,1)=-4000. DO 
X(2,1)-*12.D0 
X(3,1)-5000.D0 
X(4,1)=2.D0 


C DEFINE R AND RI MATRICES. 


R(1,1)=SB**2 
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ЕТ(1,1)-1205 0 Е( 71) 


C INITIALIZE P MATRIX 


15) 


14 


O 


DO 13 I=1,N 

DO 13 J=1,N 
P(I,J)=0.D0 
P(1,1)=SxX##2 
P(2,2)=SVX**2 
P(3,3)=SY¥**2 
Р(4,4)-5УҮ%%2 
GENERATION OFF MEASUREMENT NOISE - STORAGE. 
DO 14 I=1,M 

CALL GGNML(DS1,NS,RR) 
DO 14 J-1,NR 
VID JI TIS) 


TIME EVOLUTION 


DO 999 KK-1,NR 
T-DFLOAT(KK) 
L=KK-1 

рак ٦ 


GENERATION OFF MEASUREMENT DATA 


X1=-5000.D0+10.D0#TT 
X3=8000.D0+2000.DO*DCOS(0.035 DO*TT) 
UU=X1/X3 

Z(1,1)=DATAN2(X1,X3) 

ADD NOISE 

Y(1,1)-Z(1,1)*SB*V(1,KK) 


PROJECTION OF X: XPR = X(K+1/K) = F * X(K/K)* D*U 


CALL MM(F,X,XPR,N,N,M) 
АХ-0.р0 
ҮҮ--70.р0%р51ІМ(0.035р0%(ТТ»0.П0)) 


AY=-2.45 DO*DCOS(0.035D0*TT) 
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D(1,1)=0.D0 


D(2,1)=0.D0 
®Ю(3,1)=дү/2.0 DO 
700 ۱-7٦ 
Ë 
DO 77 I-1,N 
DO 77 J=1,M 
77 РЕЛ) ХРВ (т, Л) +. Л) 
C 


C PROJECTION OF P : PPR = P(K+1/K) = F * P(K/K) * 


CALL MM(F,P,FP,N,N,N) 
CALL MM(FP,FT,PPR,N,N,N) 
DO 78 I=1,N 
DO 78 J=1,N 

78 PPR(I,J)=PPR(I,J)+Q(I,J) 
каа 01 
X(2,1)=XPR(2,1) 
X(3,1)=XPR(3,1) 
X(4,1)=XPR(4,1) 


DO 68 I=1,N 
DO 68 J=1,N 

68 P(I,J)=PPR(I,J) 

C H- MATRIX | 
U=X(1,1)**2+X(3,1)**2 
ШИГ! 0 1 7/ 
H(1,2)=0.D0 
иг 4 88 71607 
Н(1,4)-0.р0 

C H- TRANSPOSE MATRIX 
HT(1,1)=H(1,1) 
НТ(2,1)-Н(1,2) 
HT(3,1)=H(1,3) 
HT(4,1)=H(1,4) 

C MEASUREMENT UPDATING 
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ET 9 


C 


23 


COMPUTATION OF GAIN MATRIX G=P*HT/ (H*P*HT+R) 


CALL MM(P,HT,S1,N,N,M) 
CALL MM(H,S1,$2,M,N,M) 
DO 23 I=1,M 

DO 23 J=1,M 
S2(1,J)=S2(I,J)+*R(I,J) 
S6(1,1)=1.D0/S2(1;1) 
CALL MM(S1,S6,G,N,M,M) 


СІ 77 
er تک‎ 
С 
GT(1,4)=G(4,1) 


C ERROR COVARIANCE MATRIX UPDATE: 
C P(K*1/K*1) s (I-G*H)*P(K*1/K) 
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75 


76 


CALL MM(G,H,GH,N,M,N) 

DO 73 I-1,N 

DO 73 J=1,N 

1 а 01017 
CALL MM(IGH,P,IGHP,N,N,N) 
DO 75S TIN 

DO 75 J-1,N 

PUP (T Л) TEHE 7 

DO 1. 

DO 76 J-1,N 

ECL I TU ps) 


C STATE UPDATE AT MEASUREMENT 


C 
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X(+)=X(-)+G*(Y-H(X(-))) !! BUT FOR E.K.F. 
CALL MM(H,X,HX,M,N,M) 

HH(1,1)=Y(1,1) °DATAN2(X(1.1),X( ۷ 

CALL MM(G,HH,XI,N,M,M) 

DO 80 T=1,N 

DO 80 J=1,M 

ХІІ, ШЕКТІ БІ ОН) 
Е1-р50ЕТ((Х1-Х(1,1))%%2-(Х3-Х(3,1))%%2) 


104 


107 
999 
99 


E2 (KI KCL 1)) 

Ни 06011) 

Ний: 500 le VY oAY 

WRITE (6,107)T,E2,E3,E1,X(2,1),TY 
FORMAT(6(3X(F14.4))) 

CONTINUE 

CONTINUE 

STOP 

END 


CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
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SUBROUTINE MM(A,B,C,N1,N2,N3) 
REAL*8 А(М1,М2),8(М2,М3),С(М1,М3) 
рО 100 І-1,М1 

DO 100 K=1,N3 

8“ ) 09 

DO 100 J=1,N2 | 
C(I,K)-C(I,K)*A(I,J)*B(J,K) 
RETURN 
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APPENDIX C 
B.O.T MANEUVERING TARGET SIMULATION PROGRAM. 


C 
 REAL*8 P(4,4),H(1,4) HT (4,1) ,Q(^,4) , AX, AY, 
(а, (т, В), АТМ, вт, ОА 
*RR(4000),S7(4,4),GRT(4,4) ,VXA(1,1),VYA(1,1), 
*V(1,40009) СТЕ) ТЕ 
*XI(4,1),TT,XLl,X3,EL UU, HH (15 1) ll D PEE 
*G(4,1),S6(1,1),GY(4,1),HX(1,1), TY,HHH,W, 
*GHX(4,1),F(4,4),FT(4,4),XPR(4,1),PPR(4,4), 
*FP(4,4),GT(1,4),R(1,1),GH(4,4),IGH(4,4), 
*IGHT(4,4),PUP(4,4),IGHP(4,4),IGHPT(4,4),GR(4,1) 
INTEGER N,M,NN,NR,L,KK,I,J,K,NS,HHHH ,HHHHH 
C ES 
N 
М=1 
ММ= 1 
60966066  + сее 
W=1.D0 
CECCCCEECCEECEC CEE 
NR=2000 
NS=4000 
СССССЕЄЕЕСЕСЕВЕЄЄЕ 
SB-.1D0/57.295779D0 
6666666666 ۶٢ 
SX=50. DO 
SY-50. DO 
SVX- PDO 
SVY 1 TBO 
08 09 
08 0 
HHH=0 . DO 
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10 


11 


2 
С 


DO 1 J=1,N 
DO 1 I=1,N 
Q(I,J)=0.D0 
Е(І,7)-0.р0 
NT I bO 
F(1,2)=1.D0 
F(2,2)=1.D0 
Е(3,3)=1.00 
F(3,4)=1.D0 
Оо 11 0 
DO 2 I=1,N 
DO 2 J=1,N 
FT(I,J)-F(J,I) 


GENERATION AND STORAGE OFF I.C.NOISE 
DO 10 I=1,N 

CALL GGNML (DS,NS,RR) 

DO 10 J=1,NN 
VV(J,1)=RR(J) 

MAKE MATRIX S7=IDENTITY. 
DO 11 I=1,N 

DO 11 J=1,N 
$7(1,J)=0.D0 

DO 12 I=1,N 

S7 (I,I):1.DO0 


STARI FIRST.RUN,INITIAL STATE VALUE. 


DO 99 JJ=1,NN 
X(1,1)=-4000. DO 
۸۰۰1(3 0 
X(3,1)=5000.D0 
X(^,1)-2.D0 


C DEFINE R AND RI MATRICES. 


R(1,1)=SB**2 
RI(1,1)=1.D0/R(1,1) 
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C INITIALIZE P MATRIX 
770 ٦ 
DO 13 J-1,N 

73 P(I,J)=0.D0 
РОСО 2? 
P(2,2)=SVX**2 
P(3,3)=SY**2 


б GENERATION OFF MEASURMENT NOISE - STORAGE. 
ро 14 Т=1,М 
CALL GGNML(DS1,NS,RR) 

DO l4 JUNE 

А V(I,J)=RR(J) 

C TIME EVOLUTION 
DO 999 KK=1,NR 
T=DFLOAT (KK) 

000۲0 ٢٦آ‎ 
TT DELO) 

C GENERATION OFF MEASURMENT DATA 
X1=-5000.D0+10.D0*TT 
X3=8000.D0+2000.D0*DCOS(0.035 DO*TT)+2000.D0 
۲۲ ۱'۱ 700) CO ТО 35 
Х1=-100.р0+3.р0*ТТ 
X3-8000.D0-2000.D0*DCOS(0.035 DO*TT)-10.D0*TT-«9000.D0 
IF (KK.LT.1400.AND.KK.GE.700) GO TO 33 
X1-46100.D0-30.D0*TT-6500.D0*«6500.D0 
X3=8000.D0+2000.D0*DCOS(0.035 DO*TT)+7000.D0-12000.D0 

33 CONTINUE 
И Е Ах 

C ADD NOISE 
Y(1,1)=Z(1,1)+SB*V(1,KK) 

C PROJECTION OF X: XPR = X(K+1/K) = F * X(K/K)+ D*U 
CALL MM(F,X,XPR,N,N,M) 

AX-0.DO 
VY--70.DO*DSIN(0.035DO*(TT«*0.D0)) 
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77 


AY=-2.45 DO*DCOS(0.035D0“TT) 
D(1,1)=0.D0 

D(2,1)=0.D0 

D(3,1)=AY/2.0 DO 

D(4,1)=AY 


DO 77 I=1,N 
DO 77 J=1,M 
XPR(I,J)=XPR(1I,J)+D(1I,J) 


C PROJECTION OF P : PPR = P(K+1/K) 


66 


79 


67 


86 


78 


CALL MM(F,P,FP,N,N,N) 
CALL MM(FP,FT,PPR,N,N,N) 
БЕ LT GOO) GO TO 86 
IF (KK.GT.600.AND.HHHHH.LT.1. 
IF (KK.GT.600.AND.HHHHH.GT.1. 
DO 79 I=1,N 

DO 79 J=1,N 

Q(I,J)=0.D0 

GO TO 86 

Q(1,1)=.25 DO#W 
0(1,2)-.5 ۳۲۳ 

Q(2,1)=.5 DO*W 

Q(2,2)=W 

Q(3,3)=.25 DO*W 
Q(3,4)-.5 DO*W 

0(4,3)=.5 ром 

Q(4,4)=W 

CONTINUE 

DO 78 I=1,N 

DO 78 J=1,N 
PPR(I,J)=PPR(1I,J)+Q(I,J) 
ХШ І)-ХРЕ(1,1) 
X(2,1)=XPR(2,1) 

X31 )=XPR(3,1) 
X(4,1)=XPR(4,1) 


109 


) 
) 


F * P(K/K) * 


GO TO 66 
GO TO 67 


FT * Q 


DO 68 I=1,N 
DO 68 J=1,N 

68 Р(1,3)-РРЕ(1,3) 

c H- MATRIX 
0-Х(1,1)““2-Х(3,1)952 
НТО 
H(1,2)=0.D0 
170092090117 
H(1,4)=0.D0 

C H- TRANSPOSE MATRIX 
HT(1,1)=H(1,1) 
0)2. ٦ 
HT (3 9٦ 
HT(4,1)=H(1,4) 

C UPDATING AT MEASUREMENT | 

C COMPUTATION OF GAIN MATRIX G=P*HT/ (H*P*HT+R) 
CALL MM(P,HT,S1,N,N,M) 
CALL MM(H,S1,S2,M,N,M) 
DO 23 I=1,M 
DO 23 J=1,M 

23 ОС) Е 7? 
S6(1, E1 DOSKI) 
CALL MM(S1,S6,G,N,M,M) 
CTS SG BL) 
сто) 

GTI 39603509 

88+ 77+ 
С ERROR COVARIANCE MATRIX UPDATE:P(K+1/K+1) 
(ТСН РК "ٹب‎ } 

CALL MM(G,H,GH,N,M,N) 

DO 73 I-1,N 

DO 73 J=1,N 

73 НО тена) 
CALL MM(IGH,P,IGHP,N,N,N) 
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75 


76 


DO 75 I=1,N 

DO 75 J-1,N 

РШЕ (R UJ) TGHP(I J) 
DO 76 I=1,N 

DO 76 J=1,N 
P(L,J)=PUP(I,J) 


C STATE UPDATE AT MEASURMENT 


C 


80 


107 
999 
99 


X(+)=X(-)+G*(Y-H(X(-))) !! BUT FOR E.K.F. 
CALL MM(H,X,HX,M,N,M) 

۳۳٣٠٠٠٠٢٠٠٠٠٦٦ DATAN2(X(1,1),X(3,1)) 
HHH-HH(1,1)*150.D0 

HHHH= SNGL (HHH) 

HHHHH- IABS ( HHHH ) 

CALL MM(G,HH,XI,N,M,M) 

DO 80 I-1,N 

DO 80 J-1,M 

X(I,J)sX(I,J)*XI(I,J) 
Е1-р5ОЕТ((Х1-Х(1,1))%%2»(Х3-Х(3,1))%%2) 
Е 6: (аа) 

ЕЗ-(Х3-Х(3,1)) 

77٦‏ ص۶ 

TA 1 +70 

(©,107)Т,Е2,ЕЗ,Е1,Х(2,1),Х(“4,‚1)‏ 7۲۳۶ی 
WRITE (6,107)T,X1,X3‏ 

CRITE. (6,107)T,TX,TY,HH(1,1) 
FORMAT(6(3X(F14.4))) 

CONTINUE 

CONTINUE 

STOP 

END 


ББЕБФФФЕРФЕСОСССОСССОССЕССССССССССССССССОССОСССССС 


SUBROUTINE MM(A,B,C,N1,N2,N3) 
REAL*8 A(N1,N2),B(N2,N3),C(N1,N3) 
ОН (002111 

DO 100 K-1,N3 


LEL 


100 


C(I,K)=0.D0 

DO 100 3< ¥2 

CORK ECE TAT I p O J b) 
RETURN 

END 

END 


TNI 


APPENDIX D 
PSEUDO-LINEAR FORMULATION 


If we will start we the Cartesian formulation equations: 


8(к) = h[x(k)) + n(k) (921) 
В (х(К)) = arctan[x, (k)/x; (k)] (D22) 
Е [п(к)] = 0 (D.3) 


| o Ску 2-5 


E (n(i)nG)] - | (D.4) 
О (23 
then after algebraical manipulations yield: 
0 = 2 001202) + R(k)n(k) (DS) 
where: 
H(k) - [cos$(k), Sin EO 0] (D: 6) 
R(k) yi (k) + хз (К) (D.7) 


The nonlinearity has been embeded in the measurement noise. 
ШЕ 


€(k)-R(k)n(k)-effective measurement noise at 87 &T 


143 


it can been shown [Ref. 13]. that £(k) has the following 


statistics: 
Е ГЕ(К)І = 0 (D.9) 
E (e? (k)] = R?(k)o? (k) (D.10) 


Finally the pseudo-linear model is analogous to that of 
Cartesian formulation model with the following modifica- 
tions: 

l. replacing H(k) with that given by equation ( C.6) 
2. replacing c (k) with R(k/k-1)6(k) 
3. replacing $(k)-h[x(k/k-1)] with -H(k)x(k/k-1) ' 

Detailed analysis on the subject can be found in 
[Ref. 8]. 
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